using System;
using Microsoft.Xna.Framework;

namespace Xfinity.Physics
{
    /// <summary>
    /// An ODE to simulate a simple projectile.
    /// </summary>
    public class SimpleProjectileODE : Ode
    {
        /// <summary>
        /// Constructs a new SimpleProjectile ODE.
        /// </summary>
        public SimpleProjectileODE(Vector3 initialPosition, Vector3 initialVelocity, double time)
            : base(6)
        {
            base.Time = time;
            DependantVariables[0] = initialVelocity.X;
            DependantVariables[1] = initialPosition.X;
            DependantVariables[2] = initialVelocity.Y;
            DependantVariables[3] = initialPosition.Y;
            DependantVariables[4] = initialVelocity.Z;
            DependantVariables[5] = initialPosition.Z;
        }

        #region Velocity
        /// <summary>
        /// Velocity.
        /// </summary>
        public Vector3 Velocity
        {
            get { return new Vector3((float)DependantVariables[0], (float)DependantVariables[2], (float)DependantVariables[4]); }
            set
            {
                DependantVariables[0] = value.X;
                DependantVariables[2] = value.Y;
                DependantVariables[4] = value.Z;
            }
        }

        #endregion
        #region Position
        /// <summary>
        /// Position.
        /// </summary>
        public Vector3 Position
        {
            get { return new Vector3((float)DependantVariables[1], (float)DependantVariables[3], (float)DependantVariables[5]); }
            set
            {
                DependantVariables[1] = value.X;
                DependantVariables[3] = value.Y;
                DependantVariables[5] = value.Z;
            }
        }

        #endregion
        #region Gravity
        private double gravity = 9.81;

        /// <summary>
        /// Gravity.
        /// </summary>
        public double Gravity
        {
            get { return gravity; }
            set { gravity = value; }
        }

        #endregion
        /// <summary>
        /// Updates according to a gravity-only model.
        /// </summary>
        /// <param name="gameTime">The elapsed game time.</param>
        public override void Update(GameTime gameTime)
        {
            Vector3 velocity = Velocity;
            velocity.Z += (float)(gravity * gameTime.ElapsedRealTime.TotalMilliseconds * gameTime.ElapsedRealTime.TotalMilliseconds);
            Velocity = velocity;
            Position += Velocity;
            Time += gameTime.ElapsedRealTime.TotalMilliseconds;
        }
        /// <summary>
        /// Evaluates the SimpleProjectile.
        /// </summary>
        /// <param name="time">The current time.</param>
        /// <param name="dependantVariables">All the current dependant variables.</param>
        /// <param name="deltaDependantVariables">The delta of the dependant variables.</param>
        /// <param name="deltaTime">The delta of the time.</param>
        /// <param name="dependantVariableScale">The position between 0 and 1 of the RK evaluation.</param>
        /// <returns>The new dependant variables.</returns>
        public override double[] EvaluateRightHandSide(double time, double[] dependantVariables, double[] deltaDependantVariables, double deltaTime, double dependantVariableScale)
        {
            return new double[1];
        }
    }
    public class DragAndWindProjectileODE : SimpleProjectileODE
    {
        #region Mass
        private double mass;

        /// <summary>
        /// Mass.
        /// </summary>
        public double Mass
        {
            get { return mass; }
            set { mass = value; }
        }

        #endregion
        #region Area
        private double area;

        /// <summary>
        /// Area.
        /// </summary>
        public double Area
        {
            get { return area; }
            set { area = value; }
        }

        #endregion
        #region Density
        private double density;

        /// <summary>
        /// Density.
        /// </summary>
        public double Density
        {
            get { return density; }
            set { density = value; }
        }

        #endregion
        #region DragCoefficient
        private double cd;

        /// <summary>
        /// DragCoefficient.
        /// </summary>
        public double DragCoefficient
        {
            get { return cd; }
            set { cd = value; }
        }

        #endregion
        #region Wind
        private Vector3 wind = Vector3.Zero;

        /// <summary>
        /// Wind.
        /// </summary>
        public Vector3 Wind
        {
            get { return wind; }
            set { wind = value; }
        }

        #endregion
        public DragAndWindProjectileODE(Vector3 initialPosition, Vector3 initialVelocity, double time, double mass, double area, double density, double dragCoefficient)
            : base(initialPosition, initialVelocity, time)
        {
            this.mass = mass;
            this.area = area;
            this.density = density;
            this.cd = dragCoefficient;
        }
        public override void Update(GameTime gameTime)
        {
            OdeSolver.RungeKutta4(this, gameTime.ElapsedRealTime.TotalMilliseconds);
        }
        public override double[] EvaluateRightHandSide(double time, double[] dependantVariables, double[] deltaDependantVariables, double deltaTime, double dependantVariableScale)
        {
            double[] dDependant = new double[6];
            double[] newDependant = new double[6];
            for (int i = 0; i < 6; i++)
            {
                newDependant[i] = dependantVariables[i] + dependantVariableScale * deltaDependantVariables[i];
            }
            double vx = newDependant[0] - wind.X;
            double vy = newDependant[2] - wind.Y;
            double vz = newDependant[4] - wind.Z;
            double velocityMagnitude = Math.Sqrt(vx * vx + vy * vy + vz * vz) + 1.0e-8;
            double Fd = .5 * density * area * cd * velocityMagnitude * velocityMagnitude;
            double multiplier = -deltaTime * Fd / (mass * velocityMagnitude);
            dDependant[0] = ComputeDragVelocity(vx, multiplier);
            dDependant[1] = ComputeMovement(vx, deltaTime);
            dDependant[2] = ComputeDragVelocity(vy, multiplier);
            dDependant[3] = ComputeMovement(vy, deltaTime);
            dDependant[4] = deltaTime * Gravity - ComputeDragVelocity(vz, multiplier);
            dDependant[5] = ComputeMovement(vz, deltaTime);
            return dDependant;
        }
        private static double ComputeDragVelocity(double component, double multiplier)
        {
            return -component * multiplier;
        }
        private static double ComputeMovement(double component, double deltaTime)
        {
            return deltaTime * component;
        }
    }
}